//
// Created by 刘洋 on 2024/2/12.
//
#include "FOC.h"

//供电电压
float voltage_power_supply = 12.6f;
float zero_electric_angle = 0;
int PP;
int DIR;//方向，+-1
static MotorPIN dir1a,dir1b,dir2a,dir2b;
static StepMotorPWM smp;
static BLDCMotorPWM bldcp;
uint8_t foc_modulation;

void setPhaseVoltage(float Uq, float angle_el)
{
    float Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0;
    switch (foc_modulation)
    {
        case SinePWM :
            // 正弦PWM调制
            // 逆派克+克拉克变换

            // 在0到360°之间的角度归一化
            // 只有在使用 _sin和 _cos 近似函数时才需要
            angle_el = _normalizeAngle(angle_el + zero_electric_angle);
//            printf("%.2f\r\n",angle_el);
            // 逆派克变换
            Ualpha =  -_sin(angle_el) * Uq;  // -sin(angle) * Uq;
            Ubeta =  _cos(angle_el) * Uq;    //  cos(angle) * Uq;

            // 克拉克变换
            Ua = Ualpha + voltage_power_supply/2;
            Ub = -0.5 * Ualpha  + _SQRT3_2 * Ubeta + voltage_power_supply/2;
            Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + voltage_power_supply/2;
            break;

        case SpaceVectorPWM :
            // 解释空间矢量调制(SVPWM)算法视频
            // https://www.youtube.com/watch?v=QMSWUMEAejg

            // 如果负电压的变化与相位相反
            // 角度+180度
            if(Uq < 0) angle_el += _PI;
            Uq = abs(Uq);

            // 在0到360°之间的角度归一化
            // 只有在使用 _sin和 _cos 近似函数时才需要
            angle_el = _normalizeAngle(angle_el + zero_electric_angle + _PI_2);

            // 找到我们目前所处的象限
            int sector = floor(angle_el / _PI_3) + 1;
            // 计算占空比
            float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/voltage_power_supply;
            float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/voltage_power_supply;
            // 两个版本
            // 以电压电源为中心/2
            float T0 = 1 - T1 - T2;
            // 低电源电压，拉到0
            //float T0 = 0;

            // 计算占空比（时间）
            float Ta,Tb,Tc;
            switch(sector) {
                case 1:
                    Ta = T1 + T2 + T0 / 2;
                    Tb = T2 + T0 / 2;
                    Tc = T0 / 2;
                    break;
                case 2:
                    Ta = T1 + T0 / 2;
                    Tb = T1 + T2 + T0 / 2;
                    Tc = T0 / 2;
                    break;
                case 3:
                    Ta = T0 / 2;
                    Tb = T1 + T2 + T0 / 2;
                    Tc = T2 + T0 / 2;
                    break;
                case 4:
                    Ta = T0 / 2;
                    Tb = T1 + T0 / 2;
                    Tc = T1 + T2 + T0 / 2;
                    break;
                case 5:
                    Ta = T2 + T0 / 2;
                    Tb = T0 / 2;
                    Tc = T1 + T2 + T0 / 2;
                    break;
                case 6:
                    Ta = T1 + T2 + T0 / 2;
                    Tb = T0 / 2;
                    Tc = T1 + T0 / 2;
                    break;
                default:
                    // 可能的错误状态
                    Ta = 0;
                    Tb = 0;
                    Tc = 0;
            }
            // 计算相电压和中心
            Ua = Ta*voltage_power_supply;
            Ub = Tb*voltage_power_supply;
            Uc = Tc*voltage_power_supply;
//            printf("%.2f,%.2f,%.2f\r\n",Ua,Ub,Uc);
            break;
    }

    // 设置硬件中的电压
    setBLDCPWM(Ua, Ub, Uc);
}

void setBLDCPWM(float Ua,float Ub,float Uc)
{
    float dc_a = _constrain(Ua/voltage_power_supply,0.0f,1.0f);
    float dc_b = _constrain(Ub/voltage_power_supply,0.0f,1.0f);
    float dc_c = _constrain(Uc/voltage_power_supply,0.0f,1.0f);

    __HAL_TIM_SetCompare(&bldcp.htim, bldcp.Channel1, dc_a*7200);
    __HAL_TIM_SetCompare(&bldcp.htim, bldcp.Channel2, dc_b*7200);
    __HAL_TIM_SetCompare(&bldcp.htim, bldcp.Channel3, dc_c*7200);
}
void alignSensor(int _PP,int _DIR)
{
    PP = _PP;
    DIR = _DIR;
    setBLDCFOCMode(SinePWM);
    setPhaseVoltage(3,_3PI_2);
    HAL_Delay(3000);
    zero_electric_angle=_normalizeAngle(_electricalAngle(getAngle_Without_track(),PP));
    setPhaseVoltage(0,_3PI_2);
    printf("alignSensor:%.2f\r\n",zero_electric_angle);
}
void setVbus(float voltage)
{
    voltage_power_supply = voltage;
}
void setBLDCFOCMode(FOCMode mode)
{
    foc_modulation = mode;
}
void stepMotorFOC(float Uq, float angle_el)
{
    float Ualpha = 0,Ubeta = 0;
    angle_el = _normalizeAngle(angle_el);
    float _ca = _cos(angle_el);
    float _sa = _sin(angle_el);

    Ualpha  = - _sa * Uq;  // -sin(angle) * Uq;
    Ubeta   =  _ca * Uq;    //  cos(angle) * Uq;

    setStepPWM(Ualpha,Ubeta);
}
void setStepPWM(float Ua, float Ub) {
    float duty_cycle1 = 0.0f, duty_cycle2 = 0.0f;
    // limit the voltage in driver
    Ua = _constrain(Ua, -voltage_power_supply, voltage_power_supply);
    Ub = _constrain(Ub, -voltage_power_supply, voltage_power_supply);
    printf("%.2f,%.2f\r\n",Ua,Ub);
    // hardware specific writing
    duty_cycle1 = _constrain(abs(Ua) / voltage_power_supply, 0.0, 1.0);
    duty_cycle2 = _constrain(abs(Ub) / voltage_power_supply, 0.0, 1.0);
    // phase 1 direction
    HAL_GPIO_WritePin(dir1a.GPIOx,dir1a.GPIO_Pin ,Ua >= 0 ? LOW : HIGH);
    HAL_GPIO_WritePin(dir1b.GPIOx,dir1b.GPIO_Pin, Ua <= 0 ? LOW : HIGH);
    // phase 2 direction
    HAL_GPIO_WritePin(dir2a.GPIOx, dir2a.GPIO_Pin,Ub >= 0 ? LOW : HIGH);
    HAL_GPIO_WritePin(dir2b.GPIOx,dir2b.GPIO_Pin, Ub <= 0 ? LOW : HIGH);
    // write to hardware
    __HAL_TIM_SetCompare(&smp.htim, smp.Channel1, duty_cycle1 * 7200);
    __HAL_TIM_SetCompare(&smp.htim, smp.Channel2, duty_cycle2 * 7200);
}
void setStepMotor(GPIO_TypeDef *_in1a ,uint16_t int1a,GPIO_TypeDef *_in1b ,uint16_t int1b,GPIO_TypeDef *_in2a ,uint16_t int2a,GPIO_TypeDef *_in2b ,uint16_t int2b,TIM_HandleTypeDef tim,uint8_t channel1,uint8_t channel2)
{
    // Pin initialization
    dir1a.GPIOx = _in1a;
    dir1a.GPIO_Pin = int1a;

    dir1b.GPIOx = _in1b;
    dir1b.GPIO_Pin = int1b;

    dir2a.GPIOx = _in2a;
    dir2a.GPIO_Pin = int2a;

    dir2b.GPIOx = _in2b;
    dir2b.GPIO_Pin = int2b;

    smp.htim = tim;
    smp.Channel1 = channel1;
    smp.Channel2 = channel2;
}
void setBLDCMotor(TIM_HandleTypeDef tim,uint8_t channel1,uint8_t channel2,uint8_t channel3)
{
    bldcp.htim = tim;
    bldcp.Channel1 = channel1;
    bldcp.Channel2 = channel2;
    bldcp.Channel3 = channel3;
}
float getIqId(CurrSense sensor)
{
    float I_q_M0_ori=cal_Iq_Id(sensor.current_a,sensor.current_b,DIR*_electricalAngle(getAngle_Without_track(),PP));
    float I_q_M0_flit=LowPassFilterRun(&CurrentFilter,I_q_M0_ori);
    return I_q_M0_flit;
}
float cal_Iq_Id(float current_a,float current_b,float angle_el)
{
    float I_alpha = current_a;
    float I_beta = _1_SQRT3 * current_a + _2_SQRT3 * current_b;
    angle_el = _normalizeAngle(angle_el + zero_electric_angle);
    float ct = _cos(angle_el);
    float st = _sin(angle_el);

    float I_q = I_beta * ct - I_alpha * st;
    return I_q;
}
